#include "PID.h"
#include "hardwareDef.h"

extern uint8_t flightMode;
extern float input[6];//输入数据，横滚、俯仰、油门、偏航、襟翼、AOA控制，归一法
extern float feedback[6];//横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
extern float ctrl[6];//控制数据，横滚、俯仰、油门、偏航、襟翼、AOA，归一法
extern float trueAOA;//返回真实AOA数据，单位为deg
extern float midd;//控制中间变量
extern bool rollCouldGyro;

void PID()
{
  //float x;//内外环变量传递参数
  
  switch (flightMode)
  {//根据飞行模式切换运算
    case 1://手动模式
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,1);//横滚以输入控制
      ctrl[1] = PID_AOA(input[1],feedback[3],paP,paI,paD,1);//俯仰以输入控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,1);//偏航为0
      break;

    case 2://攻角模式
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,rollCouldGyro);//横滚以横滚速率控制
      ctrl[1] = PID_AOA(input[1],feedback[3],paP,paI,paD,0);//俯仰以攻角控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,rollCouldGyro);//偏航为0
      break;

    case 3://攻角速率双闭环模式
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,0);//横滚以横滚速率控制
      midd = PID_PR(input[1],feedback[1],prP,prI,prD,0);//攻角指令由俯仰速率控制器得出
      ctrl[1] = PID_AOA(midd,feedback[3],paP,paI,paD,0);//俯仰以攻角控制器
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,0);//偏航为0
      break;

    case 4://半自稳模式
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      ctrl[0] = PID_RR(input[0],feedback[0],rrP,rrI,rrD,0);//横滚以横滚速率控制
      ctrl[1] = PID_PR(input[1],feedback[1],prP2,prI2,prD2,0);//俯仰以俯仰速率控制
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,0);//偏航为0
      break;

    default://失控或自稳模式
      //横滚角度PID
      //rP rI rD
      //俯仰角度PID
      //pP pI pD
      static float r1, p1;
      //横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
      r1 = PID_R(input[0],feedback[4],rP,rI,rD,0);//横滚角度外环由输入控制，横滚角度反馈，输出横滚角速率控制
      ctrl[0] = PID_RR(r1,feedback[0],rrP,rrI,rrD,0);//横滚速率内环由角速率控制控制，角速率反馈，输出控制值
      p1 = PID_P(input[1],feedback[5],pP,pI,pD,0);//俯仰外环由输入控制，俯仰角度反馈，输出俯仰角速率控制
      midd = PID_PR(p1,feedback[1],prP,prI,prD,0);//角速率中环由角速率控制控制，俯仰角速率反馈，输出攻角控制
      ctrl[1] = PID_AOA(midd,feedback[3],paP,paI,paD,0);//攻角以攻角控制控制，AOA反馈，输出控制值
      ctrl[3] = PID_YR(input[3],feedback[2],yrP,yrI,yrD,0);//偏航为0
      break;
  }

  /*if (abs(input[2] - ctrl[2]) >= 0.3)
  {//电机做缓冲处理
    if (input[2] > ctrl[2])
    {
      ctrl[2] = constrain(ctrl[2] + 0.3, -1, 1);
    }
    else
    {
      ctrl[2] = constrain(ctrl[2] - 0.3, -1, 1);
    }
  }
  else
  {
    ctrl[2] = input[2];
  }*/
  ctrl[2] = input[2];//电机直接控制
  ctrl[4] = input[4];//襟翼控制直接以输入控制
  //ctrl[5] = input[5] * constrain(mapF(trueAOA,-45,45,-1,1),-1,1);//AOA控制直接以输入乘攻角
}

float PID_AOA(float in,float fb,float p,float i,float d,bool rOUT)
{//攻角控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_PR(float in,float fb,float p,float i,float d,bool rOUT)
{//俯仰速率控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_RR(float in,float fb,float p,float i,float d,bool rOUT)
{//横滚速率控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_YR(float in,float fb,float p,float i,float d,bool rOUT)
{//偏航速率控制器
  float out;
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  static float isum, e_1;//积分项，上次记录
  isum = constrain(isum + e, -0.2 / i, 0.2 / i);//积分项限制到0.2
  out = p * e + i * isum + d * (e - e_1);//位置式PID
  e_1 = e;//更新上一次
  return out;
}

float PID_P(float in,float fb,float p,float i,float d,bool rOUT)
{//俯仰控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}

float PID_R(float in,float fb,float p,float i,float d,bool rOUT)
{//横滚控制器
  static float out, e_1, e_2;//输出，上次误差，上上次误差
  if (rOUT)
  {//如果原生输出
    out = in;//输出等于输入
    return out;
  }
  float e = in - fb;//计算误差
  out = constrain(out + p * (e - e_1) + i * e + d * (e - 2 * e_1 + e_2), -1, 1);//先计算增量，再相加，再钳位
  e_2 = e_1;//更新上上次
  e_1 = e;//更新上一次
  return out;
}
